# -*- coding: UTF-8 -*-
from operator import inv
import os
import cv2
import numpy as np

def rotation_vector_to_rotation_matrix(xyz : list):
    matrix = cv2.Rodrigues((xyz[3],xyz[4],xyz[5]))[0]
    print ("[")
    for i in range(3):
        print ("{},{},{},{};".format(matrix[i][0],matrix[i][1],matrix[i][2],xyz[i]))
    print ("{},{},{},{}\n];".format(0.0, 0.0, 0.0, 1.0))

c2b = [[-0.99571636,0.09214242,0.0076614,0.62653163],[0.09237527,0.99493334,0.0396794,0.24948003],[-0.00396642,0.04021715,-0.99918309,0.84846713],[ 0.,0.,0.,1.,]]
print (np.dot(c2b, [[0.3],[0.3],[0.5],[1]]))